/** @file m_matrix.cpp
	@brief Matrices of several dimensions */

#include "math/m_matrix.h"

using namespace G3;

//==============================================================================
// Constructors
//==============================================================================
template <int N, class T>
G3::MATRIX<N, T>::MATRIX () {
    GetIdentity ();
}

template <int N, class T>
G3::MATRIX<N, T>::MATRIX (const T M [N * N]) {
    Copy (M);
}

//==============================================================================
// = operators
//==============================================================================
template <int N, class T>
MATRIX<N, T>& G3::MATRIX<N, T>::operator= (const MATRIX<N, T> &M) {
    for (uint i=0; i<N*N; i++)
        m [i] = M.m [i];

    return *this;
}

//==============================================================================
// * operators
//==============================================================================
template <int N, class T>
MATRIX<N, T> G3::MATRIX<N, T>::operator* (const MATRIX<N, T> &M) const {
    MATRIX<N, T> mat;
    uint i, j, k;
    T sum = 0;

    for (i=0; i<N; i++) {
        for (j=0; j<N; j++) {
            sum = 0;
            for (k=0; k<N; k++)
                sum += m [k + i * N] * M.m [j + k * N];

            mat.m [j + i * N] = sum;
        }
    }

    return mat;
}

template <int N, class T>
MATRIX<N, T>& G3::MATRIX<N, T>::operator*= (const MATRIX<N, T> &M) {
    MATRIX<N, T> mat;
    uint i, j, k;
    T sum = 0;

    for (i=0; i<N; i++) {
        for (j=0; j<N; j++) {
            sum = 0;
            for (k=0; k<N; k++)
                sum += m [k + i * N] * M.m [j + k * N];

            m [j + i * N] = sum;
        }
    }

    return *this;
}

template <int N, class T>
MATRIX<N, T> G3::MATRIX<N, T>::operator* (const T &f) const {
    MATRIX<N, T> mat;

    for (uint i=0; i<N*N; i++)
        mat.m [i] = m [i] * f;

    return mat;
}

template <int N, class T>
MATRIX<N, T>& G3::MATRIX<N, T>::operator*= (const T &f) {
    MATRIX<N, T> mat;

    for (uint i=0; i<N*N; i++)
        m [i] *= f;

    return *this;
}

//==============================================================================
// + operators
//==============================================================================
template <int N, class T>
MATRIX<N, T> G3::MATRIX<N, T>::operator+ (const MATRIX<N, T> &M) const {
    MATRIX<N, T> mat;

    for (uint i=0; i<N*N; i++)
        mat.m [i] = m [i] + M.m [i];

    return mat;
}

//==============================================================================
// - operators
//==============================================================================
template <int N, class T>
MATRIX<N, T> G3::MATRIX<N, T>::operator- (const MATRIX<N, T> &M) const {
    MATRIX<N, T> mat;

    for (uint i=0; i<N*N; i++)
        mat.m [i] = m [i] - M.m [i];

    return mat;
}

//==============================================================================
// [] operators
//==============================================================================
template <int N, class T>
T& G3::MATRIX<N, T>::operator[] (const int &id) {
    static T temp = 0;
    if (id >= 0 && id < N * N)
        return m [id];
    return temp;
}

//==============================================================================
// Copies a 16 floats array into a matrix class
//==============================================================================
template <int N, class T>
void G3::MATRIX<N, T>::Copy (const T M [N * N]) {
    for (uint i=0; i<N*N; i++)
        m [i] = M [i];
}

//==============================================================================
// Zeroes the matrix
//==============================================================================
template <int N, class T>
void G3::MATRIX<N, T>::Zero () {
    for (uint i=0; i<N*N; i++)
        m [i] = 0;
}

//==============================================================================
// Generates an identity matrix
//==============================================================================
template <int N, class T>
void G3::MATRIX<N, T>::GetIdentity () {
    Zero ();

    for (uint i=0; i<N; i++)
        m [i + i * N] = 1;
}

//==============================================================================
// Multiplies two matrices
//==============================================================================
template <int N, class T>
MATRIX<N, T> G3::MultMatrices (const MATRIX<N, T> &M1, const MATRIX<N, T> &M2) {
    return M1 * M2;
}

//==============================================================================
// Applies a matrix to a vector
//==============================================================================
template <int N, class T>
VEC3<T> G3::ApplyMatrix (const VEC3<T> &V, const MATRIX<N, T> &M) {
    VEC3<T> v = V;

    M.TransformVector (v);

    return v;
}

template <int N, class T>
void G3::MATRIX<N, T>::TransformVector (VEC3<T> &Vect) const {
    if (N < 3)
        return;

    Vect.x = Vect.x * m [0] +
             Vect.y * m [4] +
             Vect.z * m [8];

    Vect.y = Vect.x * m [1] +
             Vect.y * m [5] +
             Vect.z * m [9];

    Vect.z = Vect.x * m [2] +
             Vect.y * m [6] +
             Vect.z * m [10];
}

template <int N, class T>
void G3::MATRIX<N, T>::TransformPoint (VEC3<T> &Point) const {
    if (N < 4)
        return;

    Point.x = Point.x * m [0] +
              Point.y * m [4] +
              Point.z * m [8] + m [12];

    Point.y = Point.x * m [1] +
              Point.y * m [5] +
              Point.z * m [9] + m [13];

    Point.z = Point.x * m [2] +
              Point.y * m [6] +
              Point.z * m [10] + m [14];
}

//==============================================================================
// Generates a translation matrix
//==============================================================================
template <int N, class T>
MATRIX<N, T> G3::TranslateMatrix (const MATRIX<N, T> &M, const VEC3<T> &t) {
    MATRIX<N, T> mat;
    mat.Translate (t);

    return M * mat;
}

template <int N, class T>
void G3::MATRIX<N, T>::Translate (const VEC3<T> &t) {
    if (N < 3)
        return;

    GetIdentity ();
    m [12] = t.x;
    m [13] = t.y;
    m [14] = t.z;
}

//==============================================================================
// Generates a scale matrix
//==============================================================================
template <int N, class T>
MATRIX<N, T> G3::ScaleMatrix (const MATRIX<N, T> &M, const VEC3<T> &s) {
    MATRIX<N, T> mat;
    mat.Scale (s);

    return M * mat;
}

template <int N, class T>
void G3::MATRIX<N, T>::Scale (const VEC3<T> &s) {
    if (N < 3)
        return;

    GetIdentity ();
    m [0 ] = s.x;
    m [5 ] = s.y;
    m [10] = s.z;
}

//==============================================================================
// Generates a pitch matrix (around X axis)
//==============================================================================
template <int N, class T>
MATRIX<N, T> G3::PitchMatrix (const MATRIX<N, T> &M, const double Angle) {
    MATRIX<N, T> mat;
    mat.RotateX (Angle);

    return M * mat;
}

template <int N, class T>
void G3::MATRIX<N, T>::RotateX (const double Angle) {
    if (N < 3)
        return;

    double sina = sin (Angle), cosa = cos (Angle);

    GetIdentity ();
    m [5 ] = cosa;
    m [6 ] = sina;
    m [9 ] =-sina;
    m [10] = cosa;
}

//==============================================================================
// Generates a yaw matrix (around Y axis)
//==============================================================================
template <int N, class T>
MATRIX<N, T> G3::YawMatrix (const MATRIX<N, T> &M, const double Angle) {
    MATRIX<N, T> mat;
    mat.RotateY (Angle);

    return M * mat;
}

template <int N, class T>
void G3::MATRIX<N, T>::RotateY (const double Angle) {
    if (N < 3)
        return;

    double sina = sin (Angle), cosa = cos (Angle);

    GetIdentity ();
    m [0 ] = cosa;
    m [2 ] = sina;
    m [8 ] =-sina;
    m [10] = cosa;
}

//==============================================================================
// Generates a roll matrix (around Z axis)
//==============================================================================
template <int N, class T>
MATRIX<N, T> G3::RollMatrix (const MATRIX<N, T> &M, const double Angle) {
    MATRIX<N, T> mat;
    mat.RotateZ (Angle);

    return M * mat;
}

template <int N, class T>
void G3::MATRIX<N, T>::RotateZ (const double Angle) {
    if (N < 3)
        return;

    double sina = sin (Angle), cosa = cos (Angle);

    GetIdentity ();
    m [0] = cosa;
    m [1] = sina;
    m [4] =-sina;
    m [5] = cosa;
}

//==============================================================================
// Generates a rotation matrix
//==============================================================================
template <int N, class T>
MATRIX<N, T> G3::RotateMatrix (const MATRIX<N, T> &M, const VEC3<T> &Angle) {
    MATRIX<N, T> mat;
    VEC3<T> Axis = Angle;
    double Mag = Axis.GetLength ();
    Axis.Normalize ();
    mat.Rotate (Axis, Mag);

    return M * mat;
}

template <int N, class T>
void G3::MATRIX<N, T>::Rotate (VEC3<T> &Axis, double Angle) {
    if (N < 3)
        return;

    double sina = sin (Angle);
    double cosa = cos (Angle);

    GetIdentity ();

    Axis.Normalize ();

    m [0]  = cosa + (1.0f - cosa) * Axis.x;
    m [1]  = (1.0f - cosa) * Axis.x * Axis.y + sina * Axis.z;
    m [2]  = (1.0f - cosa) * Axis.x * Axis.z - sina * Axis.y;
    m [3]  = 0.0f;

    m [4]  = (1.0f - cosa) * Axis.x * Axis.y - sina * Axis.z;
    m [5]  = cosa + (1.0f - cosa) * pow (Axis.y, 2);
    m [6]  = (1.0f - cosa) * Axis.y * Axis.z + sina * Axis.x;
    m [7]  = 0.0f;

    m [8]  = (1.0f - cosa) * Axis.x * Axis.z + sina * Axis.y;
    m [9]  = (1.0f - cosa) * Axis.z * Axis.z - sina * Axis.x;
    m [10] = cosa + (1.0f - cosa) * pow (Axis.z, 2);
    m [11] = 0.0f;
}

//==============================================================================
// Multiplies two 16 float array matrices
//==============================================================================
void G3::MultMatrices (float M1 [16], float M2 [16], float Dest [16]) {
    Dest[ 0] = M1[ 0] * M2[ 0] + M1[ 1] * M2[ 4] + M1[ 2] * M2[ 8] + M1[ 3] * M2[12];
    Dest[ 1] = M1[ 0] * M2[ 1] + M1[ 1] * M2[ 5] + M1[ 2] * M2[ 9] + M1[ 3] * M2[13];
    Dest[ 2] = M1[ 0] * M2[ 2] + M1[ 1] * M2[ 6] + M1[ 2] * M2[10] + M1[ 3] * M2[14];
    Dest[ 3] = M1[ 0] * M2[ 3] + M1[ 1] * M2[ 7] + M1[ 2] * M2[11] + M1[ 3] * M2[15];

    Dest[ 4] = M1[ 4] * M2[ 0] + M1[ 5] * M2[ 4] + M1[ 6] * M2[ 8] + M1[ 7] * M2[12];
    Dest[ 5] = M1[ 4] * M2[ 1] + M1[ 5] * M2[ 5] + M1[ 6] * M2[ 9] + M1[ 7] * M2[13];
    Dest[ 6] = M1[ 4] * M2[ 2] + M1[ 5] * M2[ 6] + M1[ 6] * M2[10] + M1[ 7] * M2[14];
    Dest[ 7] = M1[ 4] * M2[ 3] + M1[ 5] * M2[ 7] + M1[ 6] * M2[11] + M1[ 7] * M2[15];

    Dest[ 8] = M1[ 8] * M2[ 0] + M1[ 9] * M2[ 4] + M1[10] * M2[ 8] + M1[11] * M2[12];
    Dest[ 9] = M1[ 8] * M2[ 1] + M1[ 9] * M2[ 5] + M1[10] * M2[ 9] + M1[11] * M2[13];
    Dest[10] = M1[ 8] * M2[ 2] + M1[ 9] * M2[ 6] + M1[10] * M2[10] + M1[11] * M2[14];
    Dest[11] = M1[ 8] * M2[ 3] + M1[ 9] * M2[ 7] + M1[10] * M2[11] + M1[11] * M2[15];

    Dest[12] = M1[12] * M2[ 0] + M1[13] * M2[ 4] + M1[14] * M2[ 8] + M1[15] * M2[12];
    Dest[13] = M1[12] * M2[ 1] + M1[13] * M2[ 5] + M1[14] * M2[ 9] + M1[15] * M2[13];
    Dest[14] = M1[12] * M2[ 2] + M1[13] * M2[ 6] + M1[14] * M2[10] + M1[15] * M2[14];
    Dest[15] = M1[12] * M2[ 3] + M1[13] * M2[ 7] + M1[14] * M2[11] + M1[15] * M2[15];
}

//==============================================================================
// Dumps the matrix into log
//==============================================================================
template <int N, class T>
T G3::MATRIX<N, T>::GetFromTable (int x, int y) const {
    if (x >= 0 && x < N && y >= 0 && y < N)
        return m [y * N + x];
    return 0;
}

template <int N, class T>
void G3::MATRIX<N, T>::Dump () {
    uint i = 0, j = 0;

    MainLog.Report ("Dumping Matrix:\n");
    for (i=0; i<N; i++) {
        for (j=0; j<N; j++)
            MainLog.Report ("%.3f ", GetFromTable (i, j));
        MainLog.Report ("\n");
    }
}

//==============================================================================
// Builds a matrix from 3 vectors
//==============================================================================
template <int N, class T>
MATRIX<N, T> G3::BuildRotationMatrix (const VEC3<T> &Forward, const VEC3<T> &Up, const VEC3<T> &Right) {
    MATRIX<N, T> M;

    M.RotateByAxis (Forward, Up, Right);

    return M;
}

template <int N, class T>
void G3::MATRIX<N, T>::RotateByAxis (const VEC3<T> &Forward, const VEC3<T> &Up, const VEC3<T> &Right) {
    if (N < 3)
        return;

    VEC3<T> F = Forward, U = Up, R = Right;
    F.Normalize ();
    U.Normalize ();
    R.Normalize ();

    m [0] = R.x;
    m [4] = R.y;
    m [8] = R.z;
    m [1] = U.x;
    m [5] = U.y;
    m [9] = U.z;
    m [2] = F.x;
    m [6] = F.y;
    m [10] = F.z;
}

//==============================================================================
// Explicit template instantiation
//==============================================================================
template class G3::MATRIX<2, float>;
template class G3::MATRIX<2, double>;

template class G3::MATRIX<3, float>;
template class G3::MATRIX<3, double>;

template class G3::MATRIX<4, float>;
template class G3::MATRIX<4, double>;

template MATRIX<3, float> G3::MultMatrices (const MATRIX<3, float> &M1, const MATRIX<3, float> &M2);
template MATRIX<3, double> G3::MultMatrices (const MATRIX<3, double> &M1, const MATRIX<3, double> &M2);
template MATRIX<4, float> G3::MultMatrices (const MATRIX<4, float> &M1, const MATRIX<4, float> &M2);
template MATRIX<4, double> G3::MultMatrices (const MATRIX<4, double> &M1, const MATRIX<4, double> &M2);

template VEC3<float> G3::ApplyMatrix (const VEC3<float> &V, const MATRIX<3, float> &M);
template VEC3<double> G3::ApplyMatrix (const VEC3<double> &V, const MATRIX<3, double> &M);
template VEC3<float> G3::ApplyMatrix (const VEC3<float> &V, const MATRIX<4, float> &M);
template VEC3<double> G3::ApplyMatrix (const VEC3<double> &V, const MATRIX<4, double> &M);

template MATRIX<3, float> G3::TranslateMatrix (const MATRIX<3, float> &M, const VEC3<float> &t);
template MATRIX<3, double> G3::TranslateMatrix (const MATRIX<3, double> &M, const VEC3<double> &t);
template MATRIX<4, float> G3::TranslateMatrix (const MATRIX<4, float> &M, const VEC3<float> &t);
template MATRIX<4, double> G3::TranslateMatrix (const MATRIX<4, double> &M, const VEC3<double> &t);

template MATRIX<3, float> G3::ScaleMatrix (const MATRIX<3, float> &M, const VEC3<float> &s);
template MATRIX<3, double> G3::ScaleMatrix (const MATRIX<3, double> &M, const VEC3<double> &s);
template MATRIX<4, float> G3::ScaleMatrix (const MATRIX<4, float> &M, const VEC3<float> &s);
template MATRIX<4, double> G3::ScaleMatrix (const MATRIX<4, double> &M, const VEC3<double> &s);

template MATRIX<3, float> G3::PitchMatrix (const MATRIX<3, float> &M, const double Angle);
template MATRIX<3, double> G3::PitchMatrix (const MATRIX<3, double> &M, const double Angle);
template MATRIX<4, float> G3::PitchMatrix (const MATRIX<4, float> &M, const double Angle);
template MATRIX<4, double> G3::PitchMatrix (const MATRIX<4, double> &M, const double Angle);

template MATRIX<3, float> G3::YawMatrix (const MATRIX<3, float> &M, const double Angle);
template MATRIX<3, double> G3::YawMatrix (const MATRIX<3, double> &M, const double Angle);
template MATRIX<4, float> G3::YawMatrix (const MATRIX<4, float> &M, const double Angle);
template MATRIX<4, double> G3::YawMatrix (const MATRIX<4, double> &M, const double Angle);

template MATRIX<3, float> G3::RollMatrix (const MATRIX<3, float> &M, const double Angle);
template MATRIX<3, double> G3::RollMatrix (const MATRIX<3, double> &M, const double Angle);
template MATRIX<4, float> G3::RollMatrix (const MATRIX<4, float> &M, const double Angle);
template MATRIX<4, double> G3::RollMatrix (const MATRIX<4, double> &M, const double Angle);

template MATRIX<3, float> G3::RotateMatrix (const MATRIX<3, float> &M, const VEC3<float> &Angle);
template MATRIX<3, double> G3::RotateMatrix (const MATRIX<3, double> &M, const VEC3<double> &Angle);
template MATRIX<4, float> G3::RotateMatrix (const MATRIX<4, float> &M, const VEC3<float> &Angle);
template MATRIX<4, double> G3::RotateMatrix (const MATRIX<4, double> &M, const VEC3<double> &Angle);

template MATRIX<3, float> G3::BuildRotationMatrix (const VEC3<float> &Forward, const VEC3<float> &Up, const VEC3<float> &Right);
template MATRIX<3, double> G3::BuildRotationMatrix (const VEC3<double> &Forward, const VEC3<double> &Up, const VEC3<double> &Right);
template MATRIX<4, float> G3::BuildRotationMatrix (const VEC3<float> &Forward, const VEC3<float> &Up, const VEC3<float> &Right);
template MATRIX<4, double> G3::BuildRotationMatrix (const VEC3<double> &Forward, const VEC3<double> &Up, const VEC3<double> &Right);
